# -*- coding: utf-8 -*
#实车测试版本:收(channel 1):771 发(channel 0):772 ！！仍需要对接车速反馈的接口！！
#2024.11.15: 在can_send函数中加入了加速/转向控制开关的握手判断
#2024.11.15: 加入mqtt接收部分和aeb触发部分
#2024.11.15: 可以同时接收771或者772的CANFD数据,但是目前发送只能发一个id的数据
#2024.11.19: 解决了mqtt订阅卡循环的问题
#2024.11.19：解决ttc计算根号下为负，分母为零的报错问题
#2024.11.19: 修改了mqtt服务器ip地址为私有地址
from ctypes import *
import threading
import time
import datetime

from dbc import decode_dbc

import paho.mqtt.client as mqtt
import random
import json
from datetime import datetime, timezone
import sys
import math


lib = cdll.LoadLibrary("./libusbcanfd.so")

#读取dbc文件
dbc_path = "BYDCANFD1.0.dbc"
#dbc_path = "BYDCANFD1.0_motorola.dbc"
ms = decode_dbc(dbc_path)

#m_read = ms.get_message_by_name('CAN_303')
#m_read = ms.get_message_by_name('COM_304') #CAN_303 测试发送时填写COM_304
m_send = ms.get_message_by_name('COM_304')

CMD_CAN_FILTER                  = 0x14   # 滤波
CMD_CAN_TTX                     = 0x16   # 定时发送
CMD_CAN_TTX_CTL                 = 0x17   # 使能定时发送
CMD_CAN_TRES                    = 0x18   # CAN终端电阻
ZCAN_CMD_SET_CHNL_RECV_MERGE    = 0x32   # 设置合并接收 0:不合并接收;1:合并接收
ZCAN_CMD_GET_CHNL_RECV_MERGE    = 0x33   # 获取是否开启合并接收 0:不合并接收;1:合并接收
CMD_SET_SN                      = 0x42   # 获取SN号
CMD_GET_SN                      = 0x43   # 设置SN号
CMD_CAN_TX_TIMEOUT              = 0x44   # 发送超时
ZCAN_CMD_GET_SEND_QUEUE_SIZE    = 0x100  # 获取队列大小，uint32_t
ZCAN_CMD_GET_SEND_QUEUE_SPACE   = 0x101  # 获取队列剩余空间, uint32_t
ZCAN_CMD_SET_SEND_QUEUE_CLR     = 0x102  # 清空发送队列,1：清空
ZCAN_CMD_SET_SEND_QUEUE_EN      = 0x103  # 开启发送队列,1：使能

USBCANFD = c_uint32(33)  # 设备类型号
MAX_CHANNELS = 2         # 通道最大数量
g_thd_run = 1            # 线程运行标志
threads = []             # 接收线程
acc_enable = 0
steer_enable = 0
acc=0
steer=0
can_start = 0
v_vehicle=0
v_target = 30
brake=0
# can/canfd messgae info
class ZCAN_MSG_INFO(Structure):
    _fields_ = [("txm", c_uint, 4),  # TXTYPE:0 normal,1 once, 2self
                ("fmt", c_uint, 4),  # 0-can2.0 frame,  1-canfd frame
                ("sdf", c_uint, 1),  # 0-data frame, 1-remote frame
                ("sef", c_uint, 1),  # 0-std_frame, 1-ext_frame
                ("err", c_uint, 1),  # error flag
                ("brs", c_uint, 1),  # bit-rate switch ,0-Not speed up ,1-speed up
                ("est", c_uint, 1),  # error state
                ("tx", c_uint, 1),      # received valid, tx frame
                ("echo", c_uint, 1),    # tx valid, echo frame
                ("qsend_100us", c_uint, 1), # queue send delay unit, 1-100us, 0-ms
                ("qsend", c_uint, 1),       # send valid, queue send frame
                ("pad", c_uint, 15)]


# CAN Message Header
class ZCAN_MSG_HDR(Structure):
    _fields_ = [("ts", c_uint32),  # timestamp
                ("id", c_uint32),  # can-id
                ("inf", ZCAN_MSG_INFO),
                ("pad", c_uint16),
                ("chn", c_uint8),  # channel
                ("len", c_uint8)]  # data length

# CAN2.0-frame
class ZCAN_20_MSG(Structure):
    _fields_ = [("hdr", ZCAN_MSG_HDR),
                ("dat", c_ubyte*8)]


# CANFD frame
class ZCAN_FD_MSG(Structure):
    _fields_ = [("hdr", ZCAN_MSG_HDR),
                ("dat", c_ubyte*64)]

# filter_set
class ZCAN_FILTER(Structure):
    _fields_ = [("type", c_uint8),  # 0-std_frame,1-extend_frame
                ("pad", c_uint8*3),  # reserved
                ("sid", c_uint32),  # start_ID
                ("eid", c_uint32)]  # end_ID


class ZCAN_FILTER_TABLE(Structure):
    _fields_ = [("size", c_uint32),  # 滤波数组table实际生效部分的长度
                ("table", ZCAN_FILTER*64)]


class abit_config(Structure):
    _fields_ = [("tseg1", c_uint8),
                ("tseg2", c_uint8),
                ("sjw", c_uint8),
                ("smp", c_uint8),
                ("brp", c_uint16)]


class dbit_config(Structure):
    _fields_ = [("tseg1", c_uint8),
                ("tseg2", c_uint8),
                ("sjw", c_uint8),
                ("smp", c_uint8),
                ("brp", c_uint16)]


class ZCANFD_INIT(Structure):
    _fields_ = [("clk", c_uint32),
                ("mode", c_uint32),
                ("abit", abit_config),
                ("dbit", dbit_config)]

# Terminating resistor
class Resistance(Structure):
    _fields_ = [("res", c_uint8)]

# autosend
class ZCAN_TTX(Structure):
    _fields_ = [("interval", c_uint32),   # 定时发送周期，单位百微秒
                ("repeat", c_uint16),     # 发送次数，0等于循环发
                ("index", c_uint8),       # 定时发送列表的帧索引号，也就是第几条定时发送报文
                ("flags", c_uint8),       # 0-此帧禁用定时发送，1-此帧使能定时发送
                ("msg", ZCAN_FD_MSG)]     # CANFD帧结构体

# autosend list
class ZCAN_TTX_CFG(Structure):
    _fields_ = [("size", c_uint32),       # 实际生效的数组的长度
                ("table", ZCAN_TTX * 8)]  # 最大设置8条

############  uds resoponse #############
class PARAM_DATA(Structure):
    _pack_ = 1
    _fields_ = [("data", c_ubyte*4096)]

class DATA_BUFFER(Structure):
    _pack_ = 1
    _fields_ = [("data", c_ubyte*4096)]

class POSITIVE_DATA(Structure):
    _pack_ = 1
    _fields_ = [("sid", c_byte),
                ("data_len", c_uint),
                ]

class NEGATIVE_DATA(Structure):
    _pack_ = 1
    _fields_ = [("neg_code", c_ubyte),
                ("sid", c_ubyte),
                ("error_code", c_ubyte),
                ]

class RESPONSE_DATA(Union):
    _pack_ = 1
    _fields_ = [("positive", POSITIVE_DATA),
                ("negative", NEGATIVE_DATA),
                ("raw", c_byte*8),
                ]

class ZCAN_UDS_RESPONSE(Structure):
    _pack_ = 1
    _fields_ = [
        ("status", c_byte),  # 见ZCAN_UDS_ERROR说明
        ("reserved", c_byte*6),
        ("type", c_byte),  # 0-消极响应,1-积极响应
        ("response", RESPONSE_DATA),
    ]

#######################  uds request #################

class ZCAN_UDS_SESSION_PARAM(Structure):
    _pack_ = 1
    _fields_ = [("p2_timeout", c_uint),
                # 收到消极响应错误码为0x78后的超时时间(ms)。因PC定时器误差，建议设置不小于200ms
                ("enhanced_timeout", c_uint),
                # 接收到非本次请求服务的消极响应时是否需要判定为响应错误
                ("check_any_negative_response", c_ubyte, 1),
                # 抑制响应时是否需要等待消极响应，等待时长为响应超时时间
                ("wait_if_suppress_response", c_ubyte, 1),
                ("flag", c_ubyte, 6),  # 保留
                ("reserved0", c_byte*7)
                ]

class ZCAN_UDS_TRANS_PARAM(Structure):
    _pack_ = 1
    _fields_ = [
        ("version", c_byte),  # 0-2004版本，1-2016版本
        ("max_data_len", c_byte),  # 单帧最大数据长度, can:8, canfd:64
        # 本程序发送流控时用，连续帧之间的最小间隔, 0x00-0x7F(0ms~127ms), 0xF1-0xF9(100us~900us)
        ("local_st_min", c_byte),
        ("block_size", c_byte),  # 流控帧的块大小
        ("fill_byte", c_byte),  # 无效字节的填充数据
        ("ext_frame", c_byte),  # 0:标准帧 1:扩展帧
        # 是否忽略ECU返回流控的STmin，强制使用本程序设置的 remote_st_min
        ("is_modify_ecu_st_min", c_byte),
        # 发送多帧时用, is_ignore_ecu_st_min = 1 时有效, 0x00-0x7F(0ms~127ms), 0xF1-0xF9(100us~900us)
        ("remote_st_min", c_byte),
        ("fc_timeout", c_uint),  # 接收流控超时时间(ms), 如发送首帧后需要等待回应流控帧
        ("fill_mode", c_ubyte),  # 0-FILL_MODE_SHORT,1-FILL_MODE_NONE,2-FILL_MODE_MAX
        ("reserved0", c_byte*3),
    ]

class ZCAN_UDS_REQUEST(Structure):
    _pack_ = 1
    _fields_ = [("req_id", c_uint),  # 请求的事务ID，范围0~65535，本次请求的唯一标识
                ("channel", c_ubyte),  # 设备通道索引
                ("frame_type", c_ubyte),  # 0-can,1-CANFD,2-CANFD加速
                ("reserved0", c_byte*2),
                ("src_addr", c_uint),  # 请求ID
                ("dst_addr", c_uint),  # 响应ID
                ("suppress_response", c_byte),  # 1-抑制响应
                ("sid", c_byte),  # 请求服务id
                ("reserved1", c_byte*6),
                ("session_param", ZCAN_UDS_SESSION_PARAM),  # 会话层参数
                ("trans_param", ZCAN_UDS_TRANS_PARAM),  # 传输层参数
                ("data", POINTER(c_ubyte)),  # 请求参数
                ("data_len", c_uint),  # 请求参数长度
                ("reserved2", c_uint),
                ]

class Mqtt_Publisher:
    '''
        mqtt消息通讯接口
    '''
    def __init__(self,central_ip="w604100f.ala.dedicated.aliyun.emqxcloud.cn",port=1883,node_name='bci_',anonymous=False,timeout=60):
        '''
        :param central_ip: Broker的地址
        :param port:  端口号
        :param timeout:  连接延时
        :param node_name: 节点名称
        :param anonymous: 是否同时允许多个节点
        '''
        self.broker_ip=central_ip
        self.broker_port=port
        self.timeout=timeout
        self.connected=False
        self.node_name=node_name
        self.username = 'syb'
        self.password = '123456'
        if anonymous:
            self.node_name=self.node_name+str(random.randint(100000,999999))
        self.Start_pub()
    def Start_pub(self):
        '''
        开启publisher
        :return:
        '''
        self.client = mqtt.Client(self.node_name)     #创建客户端
        self.client.username_pw_set(self.username, self.password)
        self.client.on_connect = self.on_connect_pub  # 指定回调函数
        self.client.connect(self.broker_ip, self.broker_port, self.timeout)     #开始连接
        self.client.loop_start()    #开启一个独立的循环通讯线程。
    def Publish(self,topic,payload,qos=0,retain=False):
        '''
            发送一个mqtt消息
            :param topic: 消息名称，string类型
            :param payload: 消息内容，string类型
            :param qos: 消息等级
            :retain: 状态机消息
            :return:
        '''
        if self.connected:
            return self.client.publish(topic,payload=payload,qos=qos,retain=retain)
        else:
            raise Exception("mqtt server not connected! you may use .Start() function to connect to server firstly.")

    '''
                回调函数
    '''
    def on_connect_pub(self,client, userdata, flags, rc):
        '''
            连接到broker的回调函数
        '''
        if rc==0:
            self.connected=True

        else:
            raise Exception("Failed to connect mqtt server.")

class Mqtt_Subscriber:
    '''
        mqtt消息通讯接口
    '''
    def __init__(self,central_ip="w604100f.ala.dedicated.aliyun.emqxcloud.cn",port=1883,
                 topic_name='car_info',callback_func=None,
                 node_name='bci_',anonymous=True,timeout=60):
        '''
            :param central_ip: Broker的地址
            :param port:  端口号
            :param topic_name: 接收的消息名称
            :param callback_func: 指定回调函数
            :param timeout:  连接延时
            :param node_name: 节点名称
            :param anonymous: 是否同时允许多个节点
        '''
        self.v_obs = 0
        self.a_obs = 0
        self.v_car = 0
        self.a_car = 0
        self.D_rel = 0
        self.d_0 = 3
        self.T_delay = 1.2
        self.f_mu = 0.2
        self.a_max = 6

        self.topic=topic_name
        self.callback=callback_func
        self.broker_ip=central_ip
        self.broker_port=port
        self.timeout=timeout
        self.connected=False
        self.node_name=node_name
        self.username = 'syb'
        self.password = '123456'
        self.number=[]
        self.time_difference=[]
        if anonymous:
            self.node_name=self.node_name+str(random.randint(100000,999999))
        #self.Start_sub()
    def Start_sub(self):
        '''
        开启publisher
        :return:
        '''
        self.client = mqtt.Client(self.node_name)     #创建客户端
        self.client.username_pw_set(self.username, self.password)
        self.client.on_connect = self.on_connect_sub  # 指定回调函数
        self.client.on_message=self.default_on_message
        self.client.connect(self.broker_ip, self.broker_port, self.timeout)     #开始连接
        self.client.subscribe(self.topic)
        self.client.loop_start()    #开启一个独立的循环通讯线程。

    def ttc_calculate(self, v1, a1, v2, a2, d):
        v_rel = v2 - v1
        a_rel = a2 - a1
        #print(v_rel ** 2 + 2 * a_rel * (d - self.d_0))
        if a_rel == 0: #防止相对加速度为0
            if v_rel == 0: #如果相对速度为0，应该不会发生碰撞，给一个 > 2.6的ttc，使其不触发AEB
                ttc = 3
            else:
                ttc = (d - self.d_0)/v_rel
        else:
            ttc = (-v_rel + math.sqrt(abs(v_rel ** 2 + 2 * a_rel * (d - self.d_0)))) / (a_rel)
        return ttc

    def SeungwukMoon_distance_calculate(self, v1, v2):
        v_rel = v2 - v1
        d_br = v_rel * self.T_delay + self.f_mu * (((2 * v2 - v_rel) * v_rel) / (2 * self.a_max)) #2024.11.19：根号下的内容加了绝对值，防止为负
        return d_br
    '''
                回调函数
    '''
    def default_on_message(self,client, userdata, msg):
        '''
            默认回调函数
        '''
        global brake
        
        # print("数字：",self.number)
        message_json = msg.payload.decode('utf-8')
        # 解析JSON字符串
        message = json.loads(message_json)
        self.v_obs = message["velocity"]
        self.a_obs = message["acceleration"]
        self.v_car = 50
        self.a_car = 0
        self.D_rel = message["distance"]

        #print("Front Vehicle Speed: ", self.v_obs, "Front Vehicle Acceleration: ", self.a_obs, "Distance: ", self.D_rel)
        TTC = self.ttc_calculate(self.v_obs, self.a_obs, self.v_car, self.a_car, self.D_rel)
        D_br = self.SeungwukMoon_distance_calculate(self.v_obs, self.v_car)
        
        if self.D_rel < D_br:
            if TTC > 2.6:
                brake = 0
            elif TTC <= 2.6 and TTC > 1.6:
                print("###############AEB WARNING!###############")
                brake = 0
            elif TTC <= 1.6 and TTC > 0.6:
                print("###############AEB Activated!###############")
                brake = 0.4
            elif TTC <= 0.6:
                print("###############AEB Fully Activated!###############")
                brake = 1.0
        else:
            brake = 0
            
    def on_connect_sub(self,client, userdata, flags, rc):
        '''
            连接到broker的回调函数
        '''
        if rc == 0:
            print("Connected to MQTT Broker!")
        else:
            print("Failed to connect, return code %d\n", rc)

# 构建 CAN 帧
def construct_can_frame(id, ChIdx, pad):
    can_frame = ZCAN_20_MSG()
    can_frame.hdr.inf.txm = 0  # 0-正常发送, 2--自发自收
    can_frame.hdr.inf.fmt = 0  # 0-CAN
    can_frame.hdr.inf.sdf = 0  # 0-数据帧 1-远程帧
    can_frame.hdr.inf.sef = 0  # 0-标准帧 1- 扩展帧
    #can_frame[i].hdr.inf.echo = 1   # 发送回显

    can_frame.hdr.id = id
    can_frame.hdr.chn = ChIdx
    can_frame.hdr.len = 8

    # 队列发送
    if(pad > 0):
        can_frame.hdr.pad = pad;              # 发送后延迟 pad ms
        can_frame.hdr.inf.qsend = 1;          # 队列发送帧，仅判断首帧
        can_frame.hdr.inf.qsend_100us = 0;    # 队列发送单位，0-ms，1-100us

    for i in range(can_frame.hdr.len):
        can_frame.dat[i] = i
    
    return can_frame

def velocity_control(v,vt):
    delta=0.2*(vt-v)
    if delta>0:
        acc=min(delta,7.75)
    else:
        acc=max(delta,-5)
    v = v + acc #为了方便测试，在函数中加入了车速的输出实时根据加速度更新车速，实际是从底盘订阅车速数据，只输出加速度即可
    return acc, v 
    #return acc

# 构建 CAN 帧
def construct_canfd_frame(id, ChIdx, pad): #可以根据不同的消息类型定义不同的构建函数
    global v_vehicle
    canfd_frame = ZCAN_FD_MSG()
    canfd_frame.hdr.inf.txm = 0  # 0-正常发送
    canfd_frame.hdr.inf.fmt = 1  # 1-CANFD
    canfd_frame.hdr.inf.sdf = 0  # 0-数据帧 CANFD只有数据帧!
    canfd_frame.hdr.inf.sef = 1  # 0-标准帧, 1-扩展帧
    canfd_frame.hdr.inf.brs = 1  # 1-CANFD加速
    #canfd_frame[i].hdr.inf.echo  = 1;  # 发送回显
    
    canfd_frame.hdr.id = id
    canfd_frame.hdr.chn = ChIdx
    canfd_frame.hdr.len = 64

    # 队列发送
    if(pad > 0):
        canfd_frame.hdr.pad = pad;              # 发送后延迟 pad ms
        canfd_frame.hdr.inf.qsend = 1;          # 队列发送帧，仅判断首帧
        canfd_frame.hdr.inf.qsend_100us = 0;    # 队列发送单位，0-ms，1-100us
    
    for i in range(canfd_frame.hdr.len):
        canfd_frame.dat[i] = 0

    #canfd_frame.dat[0:4]= [151,82,251,3] #ground truth: [1,1,1.25,-300] 2024/11/12:之前测试值编译方式有问题，已重写
    #print("AEB brake: ", brake)
    if brake>0: #触发AEB
        acc = brake*(-5)
    else:
        acc,v_vehicle = velocity_control(v_vehicle,v_target)
        #acc = velocity_control(v_vehicle,v_target)
    #print("acc:",acc,"v_vehicle:",v_vehicle)
    can_message=m_send.encode({
            'AxSwt': 1,
            'SteerSwt': 1,
            'SteerAgReq': 0,
            'TarAx': acc,
            'UpprAxRate': 16,
            'LowrAxRate': -16,
            'AxCtrlSt': 1,
            'SteerAgSt': 1,
            })
    for i in range(canfd_frame.hdr.len):
        canfd_frame.dat[i] = can_message[i]
    # #print(can_message)
    return canfd_frame
 

# 通道初始化，并开启接收线程
def canfd_start(DevType, DevIdx, ChIdx):
    # 波特率结构体，数据根据zcanpro的波特率计算器得出
    global can_start
    canfd_init = ZCANFD_INIT()
    canfd_init.clk = 60000000
    canfd_init.mode = 0

    canfd_init.abit.tseg1 = 14  # 仲裁域
    canfd_init.abit.tseg2 = 3
    canfd_init.abit.sjw = 2
    canfd_init.abit.smp = 0   # smp是采样点，不涉及波特率计算
    canfd_init.abit.brp = 5

    canfd_init.dbit.tseg1 = 10  # 数据域
    canfd_init.dbit.tseg2 = 2
    canfd_init.dbit.sjw = 2
    canfd_init.dbit.smp = 0
    canfd_init.dbit.brp = 1

    # 初始化通道
    ret = lib.VCI_InitCAN(DevType, DevIdx, ChIdx, byref(canfd_init))
    if ret == 0:
        print("InitCAN(%d) fail" % i)
        exit(0)
    else:
        print("InitCAN(%d) success" % i)

    # 使能终端电阻
    Res = c_uint8(1)
    lib.VCI_SetReference(DevType, DevIdx, ChIdx, CMD_CAN_TRES, byref(Res))

    # filter_set = 0x14
    # filter_table = ZCAN_FILTER_TABLE()
    # memset(byref(filter_table), 0, sizeof(filter_table))
    # filter_table.size = sizeof(ZCAN_FILTER)*1  # 设置一组滤波参数
    # filter_table.table[0].type = 0
    # filter_table.table[0].sid = 0x0
    # filter_table.table[0].eid = 0x7FF  # 设置滤波标准帧，范围0~0x7FF
    # ret = lib.VCI_SetReference(
    #     Devicetype, DeviceIndex, Channel, filter_set, byref(filter_table))
    # 
    # wait_tx = 0x44  # tx_timeout
    # tx_timeout = ZCAN_tx_timeout(200)
    # ret = lib.VCI_SetReference(
    #     Devicetype, DeviceIndex, Channel, wait_tx, byref(tx_timeout))

    # 启动通道
    ret = lib.VCI_StartCAN(DevType, DevIdx, ChIdx)
    can_start = ret
    if ret == 0:
        print("StartCAN(%d) fail" % i)
        exit(0)
    else:
        print("StartCAN(%d) success" % i)
    
    thread_rx_771 = threading.Thread(target=rx_thread, args=(DevType, DevIdx, i,771))
    threads.append(thread_rx_771) # 独立接收线程
    thread_rx_771.start()

    # thread_rx_772 = threading.Thread(target=rx_thread, args=(DevType, DevIdx, i,772)) #接收指定id的CANFD消息 771 测试发送时填写772
    # threads.append(thread_rx_772) # 独立接收线程
    # thread_rx_772.start()



# 接收线程
def rx_thread(DevType, DevIdx, ChIdx,id): #接收线程写在canfd_start内
    global g_thd_run
    global acc_enable
    global steer_enable
    global acc
    global steer
    global v_vehicle
    while g_thd_run == 1:
        #time.sleep(0.1) 
        # count = lib.VCI_GetReceiveNum(DevType, DevIdx, ChIdx) # CAN 报文数量
        # if count > 0:
        #     can_data = (ZCAN_20_MSG * count)()
        #     rcount = lib.VCI_Receive(DevType, DevIdx, ChIdx, byref(can_data), count, 100) # 读报文
        #     for i in range(rcount):
        #         print("[%u] chn: %d " %(can_data[i].hdr.ts, ChIdx), end='')
        #         print("TX  " if can_data[i].hdr.inf.tx == 1 else "RX  ", end='')       # 判断是否回显
        #         print("CAN ID: 0x%x "%(can_data[i].hdr.id & 0x1FFFFFFF), end='')
        #         print("扩展帧  " if can_data[i].hdr.inf.sef == 1 else "标准帧  ", end='')
        #         print("Data: ", end='')
        #         if(can_data[i].hdr.inf.sdf == 0):   #数据帧
        #             for j in range(can_data[i].hdr.len):
        #                 print("%02x " % can_data[i].dat[j], end='')
        #         print("")

        count = lib.VCI_GetReceiveNum(DevType, DevIdx, (0x80000000 + ChIdx)) # CANFD 报文数量 返回值为该通道的接收缓存的CAN/CANFD帧的数量。
        if count > 0:
            canfd_data = (ZCAN_FD_MSG * count)()
            rcount = lib.VCI_ReceiveFD(DevType, DevIdx, ChIdx, byref(canfd_data), count, 100) #返回实际从接收缓存读取的CANFD帧的数目
            for i in range(rcount):
                #print("CANFD ID:",canfd_data[i].hdr.id)
                if canfd_data[i].hdr.id == id: #只接收指定的id的CANFD帧
                    print("[%u] chn: %d " %(canfd_data[i].hdr.ts, ChIdx), end='')
                    print("TX  " if canfd_data[i].hdr.inf.tx == 1 else "RX  ", end='')
                    print("CANFD加速 " if canfd_data[i].hdr.inf.brs == 1 else "CANFD  ", end='')
                    print("ID: 0x%x "%(canfd_data[i].hdr.id & 0x1FFFFFFF), end='')
                    print("扩展帧  " if canfd_data[i].hdr.inf.sef == 1 else "标准帧  ", end='')
                    data=[]
                    physical_value = dict()
                    print("Data: ", end='')
                    for j in range(canfd_data[i].hdr.len):
                         print("%02x " % canfd_data[i].dat[j], end='') #打印每一条CANFD报文的每一位数据
                         data.append(canfd_data[i].dat[j])
                    if id == 771:
                        m_read = ms.get_message_by_name('CAN_303')
                    if id == 772:
                        m_read = ms.get_message_by_name('COM_304')
                    physical_value=m_read.decode(data)
                    if id==771:
                        acc_enable = physical_value['AxCtrlAvl']#加速度控制开关
                        steer_enable = physical_value['SteerAgCtrlAvl']#转向控制开关
                        acc=physical_value['ActAxFb']#目标加速度反馈
                        steer=physical_value['ActSteerAg']#目标转向角反馈
                        #v_vehicle= #车速反馈
                    #print("\n")
                    print("Pyhsical value: ", physical_value)


# 发送数据
def can_send(DevType, DevIdx, ChIdx):#后续可以把这个放入控制指令的ros消息回调函数中
    send_num = 10

    # CAN
    # can_frame = (ZCAN_20_MSG * send_num)()
    # for i in range(send_num):
    #     can_frame[i] = construct_can_frame(i, ChIdx, 0)
    # ret = lib.VCI_Transmit(DevType, DevIdx, ChIdx, byref(can_frame), 10)
    # print("Transmit num is:%d" % ret)

    # CANFD
    canfd_frame = (ZCAN_FD_MSG * send_num)()
    #for i in range(send_num):
    while(can_start):
    #while (can_start and acc_enable==1 and steer_enable==1):
        #canfd_frame[0] = construct_canfd_frame(771, ChIdx, 0) #几个不同id的canfd消息组成一个一起发出
        canfd_frame[1] = construct_canfd_frame(772, ChIdx, 0)
        ret = lib.VCI_TransmitFD(DevType, DevIdx, ChIdx, byref(canfd_frame), 10)
        #print("TransmitFD num is:%d" % ret)


# 主函数
if __name__ == "__main__":
    DEVICE_INDEX = c_uint32(0)  # 设备索引
    CHANNELS_INDEX = 0          # 测试发送的通道号
    #IsMerge = 0;                # 是否合并接收

    # 打开设备
    ret = lib.VCI_OpenDevice(USBCANFD, DEVICE_INDEX, 0)
    if ret == 0:
        print("Open device fail")
        exit(0)
    else:
        print("Open device success")

    # 打开通道
    for i in range(MAX_CHANNELS):
        canfd_start(USBCANFD, DEVICE_INDEX, i)  # 初始化通道，并且开启接收线程

    #独立发送线程，测试发送数据
    thread_tx = threading.Thread(target=can_send, args=(USBCANFD, DEVICE_INDEX, CHANNELS_INDEX,))
    threads.append(thread_tx) 
    thread_tx.start()
    
    print("MQTTTTSub")
    #mqtt
    s = Mqtt_Subscriber()
    thread_mqttsub = threading.Thread(target=s.Start_sub)
    threads.append(thread_mqttsub)
    thread_mqttsub.start()
    
    print("MQTTTTPub")
    p=Mqtt_Publisher()
    while not p.connected:
        #print("waiting for mqtt server connected...")
        pass
    for num in range(1001):
        #start_time = time.time()
        timestamp = datetime.now(timezone.utc)
        timestamp_str = timestamp.isoformat()
        #timestamp = datetime.strptime(timestamp_str, "%Y-%m-%dT%H:%M:%S.%f%z")
        #print("消息内容：",timestamp_str)

        message = {
            "acc_enable": acc_enable, #加速度控制开关
            "steer_enable":steer_enable, #转向控制开关
            "acc": acc, #目标加速度反馈
            "steer": steer, #目标转向角反馈
        }
        message_json = json.dumps(message)

        # print(f"代码执行时间: {execution_time} 秒")
        p.Publish('canfd_data',message_json)
        time.sleep(0.1)

    # 阻塞等待
    input()
    g_thd_run = 0
    
    # 等待所有线程完成
    for thread in threads:
        thread.join()

    for i in range(MAX_CHANNELS):
        ret = lib.VCI_ResetCAN(USBCANFD, DEVICE_INDEX, i)
        if ret == 0:
            print("ResetCAN(%d) fail" % i)
        else:
            print("ResetCAN(%d) success!" % i)

    ret = lib.VCI_CloseDevice(USBCANFD, DEVICE_INDEX)
    if ret == 0:
        print("Closedevice fail!")
    else:
        print("Closedevice success")
    del lib
